#include "matrix.h"
#include <stdint.h>

static inline int
_inverse_scale(const int *m , int *o) {
	if (m[0] == 0 || m[3] == 0)
		return 1;
	o[0] = (1024 * 1024) / m[0];
	o[1] = 0;
	o[2] = 0;
	o[3] = (1024 * 1024) / m[3];
	o[4] = - (m[4] * o[0]) / 1024;
	o[5] = - (m[5] * o[3]) / 1024;
	return 0;
}

static inline int
_inverse_rot(const int *m, int *o) {
	if (m[1] == 0 || m[2] == 0)
		return 1;
	o[0] = 0;
	o[1] = (1024 * 1024) / m[2];
	o[2] = (1024 * 1024) / m[1];
	o[3] = 0;
	o[4] = - (m[5] * o[2]) / 1024;
	o[5] = - (m[4] * o[1]) / 1024;
	return 0;
}

int
matrix_inverse(const struct matrix *mm, struct matrix *mo) {
	const int *m = mm->m;
	int *o = mo->m;
	if (m[1] == 0 && m[2] == 0) {
		return _inverse_scale(m,o);
	}
	if (m[0] == 0 && m[3] == 0) {
		return _inverse_rot(m,o);
	}
	int t = m[0] * m[3] - m[1] * m[2] ;
	if (t == 0)
		return 1;
	o[0] = (int32_t)((int64_t)m[3] * (1024 * 1024) / t);
	o[1] = (int32_t)(- (int64_t)m[1] * (1024 * 1024) / t);
	o[2] = (int32_t)(- (int64_t)m[2] * (1024 * 1024) / t);
	o[3] = (int32_t)((int64_t)m[0] * (1024 * 1024) / t);
	o[4] = - (m[4] * o[0] + m[5] * o[2]) / 1024;
	o[5] = - (m[4] * o[1] + m[5] * o[3]) / 1024;
	return 0;
}

// SRT to matrix

static inline int
icost(int dd) {
	static int t[2048] = {
		1024,1024,1024,1024,1024,1024,1024,1024,1024,1024,1024,1024,1024,1024,1024,1024,
		1024,1024,1024,1024,1024,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,
		1023,1023,1023,1023,1022,1022,1022,1022,1022,1022,1022,1022,1022,1022,1021,1021,
		1021,1021,1021,1021,1021,1021,1020,1020,1020,1020,1020,1020,1020,1020,1019,1019,
		1019,1019,1019,1019,1018,1018,1018,1018,1018,1018,1017,1017,1017,1017,1017,1016,
		1016,1016,1016,1016,1016,1015,1015,1015,1015,1014,1014,1014,1014,1014,1013,1013,
		1013,1013,1012,1012,1012,1012,1011,1011,1011,1011,1010,1010,1010,1010,1009,1009,
		1009,1009,1008,1008,1008,1008,1007,1007,1007,1006,1006,1006,1006,1005,1005,1005,
		1004,1004,1004,1003,1003,1003,1002,1002,1002,1001,1001,1001,1000,1000,1000,999,
		999,999,998,998,998,997,997,997,996,996,996,995,995,994,994,994,
		993,993,993,992,992,991,991,991,990,990,989,989,989,988,988,987,
		987,986,986,986,985,985,984,984,983,983,983,982,982,981,981,980,
		980,979,979,979,978,978,977,977,976,976,975,975,974,974,973,973,
		972,972,971,971,970,970,969,969,968,968,967,967,966,966,965,965,
		964,964,963,963,962,961,961,960,960,959,959,958,958,957,957,956,
		955,955,954,954,953,953,952,951,951,950,950,949,948,948,947,947,
		946,945,945,944,944,943,942,942,941,941,940,939,939,938,937,937,
		936,936,935,934,934,933,932,932,931,930,930,929,928,928,927,926,
		926,925,924,924,923,922,922,921,920,920,919,918,917,917,916,915,
		915,914,913,913,912,911,910,910,909,908,907,907,906,905,905,904,
		903,902,902,901,900,899,899,898,897,896,896,895,894,893,893,892,
		891,890,889,889,888,887,886,885,885,884,883,882,882,881,880,879,
		878,878,877,876,875,874,873,873,872,871,870,869,868,868,867,866,
		865,864,863,863,862,861,860,859,858,857,857,856,855,854,853,852,
		851,851,850,849,848,847,846,845,844,843,843,842,841,840,839,838,
		837,836,835,834,834,833,832,831,830,829,828,827,826,825,824,823,
		822,822,821,820,819,818,817,816,815,814,813,812,811,810,809,808,
		807,806,805,804,803,802,801,800,799,798,798,797,796,795,794,793,
		792,791,790,789,788,787,786,785,784,783,782,780,779,778,777,776,
		775,774,773,772,771,770,769,768,767,766,765,764,763,762,761,760,
		759,758,757,756,755,753,752,751,750,749,748,747,746,745,744,743,
		742,741,739,738,737,736,735,734,733,732,731,730,729,727,726,725,
		724,723,722,721,720,719,717,716,715,714,713,712,711,709,708,707,
		706,705,704,703,702,700,699,698,697,696,695,693,692,691,690,689,
		688,687,685,684,683,682,681,679,678,677,676,675,674,672,671,670,
		669,668,666,665,664,663,662,660,659,658,657,656,654,653,652,651,
		650,648,647,646,645,644,642,641,640,639,637,636,635,634,632,631,
		630,629,628,626,625,624,623,621,620,619,618,616,615,614,613,611,
		610,609,607,606,605,604,602,601,600,599,597,596,595,593,592,591,
		590,588,587,586,584,583,582,581,579,578,577,575,574,573,572,570,
		569,568,566,565,564,562,561,560,558,557,556,554,553,552,550,549,
		548,547,545,544,543,541,540,539,537,536,535,533,532,530,529,528,
		526,525,524,522,521,520,518,517,516,514,513,512,510,509,507,506,
		505,503,502,501,499,498,497,495,494,492,491,490,488,487,485,484,
		483,481,480,479,477,476,474,473,472,470,469,467,466,465,463,462,
		460,459,458,456,455,453,452,451,449,448,446,445,443,442,441,439,
		438,436,435,434,432,431,429,428,426,425,424,422,421,419,418,416,
		415,414,412,411,409,408,406,405,403,402,401,399,398,396,395,393,
		392,390,389,388,386,385,383,382,380,379,377,376,374,373,371,370,
		369,367,366,364,363,361,360,358,357,355,354,352,351,349,348,346,
		345,343,342,341,339,338,336,335,333,332,330,329,327,326,324,323,
		321,320,318,317,315,314,312,311,309,308,306,305,303,302,300,299,
		297,296,294,293,291,290,288,287,285,284,282,281,279,278,276,275,
		273,272,270,269,267,266,264,263,261,259,258,256,255,253,252,250,
		249,247,246,244,243,241,240,238,237,235,234,232,230,229,227,226,
		224,223,221,220,218,217,215,214,212,211,209,207,206,204,203,201,
		200,198,197,195,194,192,191,189,187,186,184,183,181,180,178,177,
		175,174,172,170,169,167,166,164,163,161,160,158,156,155,153,152,
		150,149,147,146,144,142,141,139,138,136,135,133,132,130,128,127,
		125,124,122,121,119,118,116,114,113,111,110,108,107,105,103,102,
		100,99,97,96,94,93,91,89,88,86,85,83,82,80,78,77,
		75,74,72,71,69,67,66,64,63,61,60,58,57,55,53,52,
		50,49,47,46,44,42,41,39,38,36,35,33,31,30,28,27,
		25,24,22,20,19,17,16,14,13,11,9,8,6,5,3,2,
		0,-1,-2,-4,-5,-7,-8,-10,-12,-13,-15,-16,-18,-19,-21,-23,
		-24,-26,-27,-29,-30,-32,-34,-35,-37,-38,-40,-41,-43,-45,-46,-48,
		-49,-51,-52,-54,-56,-57,-59,-60,-62,-63,-65,-66,-68,-70,-71,-73,
		-74,-76,-77,-79,-81,-82,-84,-85,-87,-88,-90,-92,-93,-95,-96,-98,
		-99,-101,-102,-104,-106,-107,-109,-110,-112,-113,-115,-117,-118,-120,-121,-123,
		-124,-126,-127,-129,-131,-132,-134,-135,-137,-138,-140,-141,-143,-145,-146,-148,
		-149,-151,-152,-154,-155,-157,-159,-160,-162,-163,-165,-166,-168,-169,-171,-173,
		-174,-176,-177,-179,-180,-182,-183,-185,-186,-188,-190,-191,-193,-194,-196,-197,
		-199,-200,-202,-203,-205,-206,-208,-210,-211,-213,-214,-216,-217,-219,-220,-222,
		-223,-225,-226,-228,-229,-231,-233,-234,-236,-237,-239,-240,-242,-243,-245,-246,
		-248,-249,-251,-252,-254,-255,-257,-258,-260,-262,-263,-265,-266,-268,-269,-271,
		-272,-274,-275,-277,-278,-280,-281,-283,-284,-286,-287,-289,-290,-292,-293,-295,
		-296,-298,-299,-301,-302,-304,-305,-307,-308,-310,-311,-313,-314,-316,-317,-319,
		-320,-322,-323,-325,-326,-328,-329,-331,-332,-334,-335,-337,-338,-340,-341,-342,
		-344,-345,-347,-348,-350,-351,-353,-354,-356,-357,-359,-360,-362,-363,-365,-366,
		-368,-369,-370,-372,-373,-375,-376,-378,-379,-381,-382,-384,-385,-387,-388,-389,
		-391,-392,-394,-395,-397,-398,-400,-401,-402,-404,-405,-407,-408,-410,-411,-413,
		-414,-415,-417,-418,-420,-421,-423,-424,-425,-427,-428,-430,-431,-433,-434,-435,
		-437,-438,-440,-441,-442,-444,-445,-447,-448,-450,-451,-452,-454,-455,-457,-458,
		-459,-461,-462,-464,-465,-466,-468,-469,-471,-472,-473,-475,-476,-478,-479,-480,
		-482,-483,-484,-486,-487,-489,-490,-491,-493,-494,-496,-497,-498,-500,-501,-502,
		-504,-505,-506,-508,-509,-511,-512,-513,-515,-516,-517,-519,-520,-521,-523,-524,
		-525,-527,-528,-529,-531,-532,-534,-535,-536,-538,-539,-540,-542,-543,-544,-546,
		-547,-548,-549,-551,-552,-553,-555,-556,-557,-559,-560,-561,-563,-564,-565,-567,
		-568,-569,-571,-572,-573,-574,-576,-577,-578,-580,-581,-582,-583,-585,-586,-587,
		-589,-590,-591,-592,-594,-595,-596,-598,-599,-600,-601,-603,-604,-605,-606,-608,
		-609,-610,-612,-613,-614,-615,-617,-618,-619,-620,-622,-623,-624,-625,-627,-628,
		-629,-630,-631,-633,-634,-635,-636,-638,-639,-640,-641,-643,-644,-645,-646,-647,
		-649,-650,-651,-652,-653,-655,-656,-657,-658,-659,-661,-662,-663,-664,-665,-667,
		-668,-669,-670,-671,-673,-674,-675,-676,-677,-678,-680,-681,-682,-683,-684,-686,
		-687,-688,-689,-690,-691,-692,-694,-695,-696,-697,-698,-699,-701,-702,-703,-704,
		-705,-706,-707,-708,-710,-711,-712,-713,-714,-715,-716,-718,-719,-720,-721,-722,
		-723,-724,-725,-726,-728,-729,-730,-731,-732,-733,-734,-735,-736,-737,-738,-740,
		-741,-742,-743,-744,-745,-746,-747,-748,-749,-750,-751,-752,-754,-755,-756,-757,
		-758,-759,-760,-761,-762,-763,-764,-765,-766,-767,-768,-769,-770,-771,-772,-773,
		-774,-775,-776,-777,-778,-779,-781,-782,-783,-784,-785,-786,-787,-788,-789,-790,
		-791,-792,-793,-794,-795,-796,-797,-797,-798,-799,-800,-801,-802,-803,-804,-805,
		-806,-807,-808,-809,-810,-811,-812,-813,-814,-815,-816,-817,-818,-819,-820,-821,
		-821,-822,-823,-824,-825,-826,-827,-828,-829,-830,-831,-832,-833,-833,-834,-835,
		-836,-837,-838,-839,-840,-841,-842,-842,-843,-844,-845,-846,-847,-848,-849,-850,
		-850,-851,-852,-853,-854,-855,-856,-856,-857,-858,-859,-860,-861,-862,-862,-863,
		-864,-865,-866,-867,-867,-868,-869,-870,-871,-872,-872,-873,-874,-875,-876,-877,
		-877,-878,-879,-880,-881,-881,-882,-883,-884,-884,-885,-886,-887,-888,-888,-889,
		-890,-891,-892,-892,-893,-894,-895,-895,-896,-897,-898,-898,-899,-900,-901,-901,
		-902,-903,-904,-904,-905,-906,-906,-907,-908,-909,-909,-910,-911,-912,-912,-913,
		-914,-914,-915,-916,-916,-917,-918,-919,-919,-920,-921,-921,-922,-923,-923,-924,
		-925,-925,-926,-927,-927,-928,-929,-929,-930,-931,-931,-932,-933,-933,-934,-935,
		-935,-936,-936,-937,-938,-938,-939,-940,-940,-941,-941,-942,-943,-943,-944,-944,
		-945,-946,-946,-947,-947,-948,-949,-949,-950,-950,-951,-952,-952,-953,-953,-954,
		-954,-955,-956,-956,-957,-957,-958,-958,-959,-959,-960,-960,-961,-962,-962,-963,
		-963,-964,-964,-965,-965,-966,-966,-967,-967,-968,-968,-969,-969,-970,-970,-971,
		-971,-972,-972,-973,-973,-974,-974,-975,-975,-976,-976,-977,-977,-978,-978,-978,
		-979,-979,-980,-980,-981,-981,-982,-982,-982,-983,-983,-984,-984,-985,-985,-985,
		-986,-986,-987,-987,-988,-988,-988,-989,-989,-990,-990,-990,-991,-991,-992,-992,
		-992,-993,-993,-993,-994,-994,-995,-995,-995,-996,-996,-996,-997,-997,-997,-998,
		-998,-998,-999,-999,-999,-1000,-1000,-1000,-1001,-1001,-1001,-1002,-1002,-1002,-1003,-1003,
		-1003,-1004,-1004,-1004,-1005,-1005,-1005,-1005,-1006,-1006,-1006,-1007,-1007,-1007,-1007,-1008,
		-1008,-1008,-1008,-1009,-1009,-1009,-1009,-1010,-1010,-1010,-1010,-1011,-1011,-1011,-1011,-1012,
		-1012,-1012,-1012,-1013,-1013,-1013,-1013,-1013,-1014,-1014,-1014,-1014,-1015,-1015,-1015,-1015,
		-1015,-1015,-1016,-1016,-1016,-1016,-1016,-1017,-1017,-1017,-1017,-1017,-1017,-1018,-1018,-1018,
		-1018,-1018,-1018,-1019,-1019,-1019,-1019,-1019,-1019,-1019,-1019,-1020,-1020,-1020,-1020,-1020,
		-1020,-1020,-1020,-1021,-1021,-1021,-1021,-1021,-1021,-1021,-1021,-1021,-1021,-1022,-1022,-1022,
		-1022,-1022,-1022,-1022,-1022,-1022,-1022,-1022,-1022,-1022,-1022,-1022,-1023,-1023,-1023,-1023,
		-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,-1023,
	};

	// using symmetry to reduce the size of lookup table
	int x = dd & 0x800;
	dd = dd & 0x7ff;	// % 2048
	if(x) {
		return -t[dd];
	} else {
		return t[dd];
	}
}

static inline int
icosd(int d) {
	return icost(d);
}

static inline int
isind(int d) {
	return icosd(1024 - d);
}

static inline void
rot_mat(int *m, int d) {
	if (d==0)
		return;
	int cosd = icosd(d);
	int sind = isind(d);

	int m0_cosd = m[0] * cosd;
	int m0_sind = m[0] * sind;
	int m1_cosd = m[1] * cosd;
	int m1_sind = m[1] * sind;
	int m2_cosd = m[2] * cosd;
	int m2_sind = m[2] * sind;
	int m3_cosd = m[3] * cosd;
	int m3_sind = m[3] * sind;
	int m4_cosd = m[4] * cosd;
	int m4_sind = m[4] * sind;
	int m5_cosd = m[5] * cosd;
	int m5_sind = m[5] * sind;

	m[0] = (m0_cosd - m1_sind) /1024;
	m[1] = (m0_sind + m1_cosd) /1024;
	m[2] = (m2_cosd - m3_sind) /1024;
	m[3] = (m2_sind + m3_cosd) /1024;
	m[4] = (m4_cosd - m5_sind) /1024;
	m[5] = (m4_sind + m5_cosd) /1024;
}

static inline void
scale_mat(int *m, int sx, int sy) {
	if (sx != 1024) {
		m[0] = m[0] * sx / 1024;
		m[2] = m[2] * sx / 1024;
		m[4] = m[4] * sx / 1024;
	}
	if (sy != 1024) {
		m[1] = m[1] * sy / 1024;
		m[3] = m[3] * sy / 1024;
		m[5] = m[5] * sy / 1024;
	}
}

void
matrix_srt(struct matrix *mm, const struct srt *srt) {
	if (!srt) {
		return;
	}
	scale_mat(mm->m, srt->scalex, srt->scaley);
	rot_mat(mm->m, srt->rot);
	mm->m[4] += srt->offx;
	mm->m[5] += srt->offy;
}

void
matrix_rot(struct matrix *m, int rot) {
	rot_mat(m->m, rot);
}

void
matrix_sr(struct matrix *mat, int sx, int sy, int d) {
	int *m = mat->m;
	int cosd = icosd(d);
	int sind = isind(d);

	int m0_cosd = sx * cosd;
	int m0_sind = sx * sind;
	int m3_cosd = sy * cosd;
	int m3_sind = sy * sind;

	m[0] = m0_cosd /1024;
	m[1] = m0_sind /1024;
	m[2] = -m3_sind /1024;
	m[3] = m3_cosd /1024;
}

void
matrix_rs(struct matrix *mat, int sx, int sy, int d) {
	int *m = mat->m;
	int cosd = icosd(d);
	int sind = isind(d);

	int m0_cosd = sx * cosd;
	int m0_sind = sx * sind;
	int m3_cosd = sy * cosd;
	int m3_sind = sy * sind;

	m[0] = m0_cosd /1024;
	m[1] = m3_sind /1024;
	m[2] = -m0_sind /1024;
	m[3] = m3_cosd /1024;
}

void
matrix_scale(struct matrix *m, int sx, int sy) {
	scale_mat(m->m, sx, sy);
}

